#include "DJI_Motor.h"

int turn(int start, int end)
{
    // 0~8191返回顺时针角度
    if (end >= start)
        return end - start;
    else
        return end - start + 8192;
}
void Limit(float &num, const float &max)
{
    if (num > max)
        num = max;
    else if (num < -max)
        num = -max;
}

void DJI_Motor::Angle_PID(float Kp, float Kd)
{
    pos_err[1] = pos_err[0];
    pos_err[0] = target_angle - sum_Angle.load();
    float Pout = Kp * pos_err[0];
    float Dout = Kd * rec_velocity.load();
    float Out = Pout + Dout;
    Limit(Out, MaxCurrent);
    set_current(Out);
}
void DJI_Motor::Vel_PID(float Kp, float Ki)
{
    // vel_err[1] = vel_err[0];
    vel_err[0] = target_velocity - rec_velocity.load();
    float Pout = Kp * vel_err[0];
    Limit(Pout, 20000);

    Iout = Iout + Ki * vel_err[0];
    Limit(Iout, 2000);

    float Out = Pout + Iout;
    Limit(Out, MaxCurrent);
    // std::cout << "Pout: " << Pout << std::endl;
    // std::cout << "Iout: " << Iout << std::endl;
    // std::cout << "Out: " << Out << std::endl;
    set_current(Out);
}
void DJI_Motor::Cascade_PID(float out_Kp, float out_Kd, float in_Kp, float in_Ki)
{
    pos_err[1] = pos_err[0];
    pos_err[0] = target_angle - sum_Angle.load();

    float Pout = out_Kp * pos_err[0];
    float Dout = out_Kd * pos_err[0] - pos_err[1];
    float out_Out = Pout + Dout;
    // std::cout << "out_P" << Pout << " Dout: " << Dout << " out_Out: " << out_Out << std::endl;
    Limit(out_Out, target_velocity);

    Pout = in_Kp * (out_Out - rec_velocity.load());
    Iout += in_Ki * (out_Out - rec_velocity.load());
    float in_Out = Pout + Iout;
    Limit(in_Out, MaxCurrent);
    set_current(in_Out);
    // std::cout << "in_P" << Pout << " Iout: " << Iout << " in_Out: " << in_Out << std::endl;
}

void m2006::modify(const uint8_t *data)
{
    // std::cout << "m2006 modify" << std::endl;
    rec_angle = ((int16_t)data[0] << 8) | (int16_t)data[1];
    rec_velocity = ((int16_t)data[2] << 8) | (int16_t)data[3];
    rec_torque = ((int16_t)data[4] << 8) | (int16_t)data[5];
    // rec_temp     = (int8_t)data[6];.

    if (last_Angle < 0)
        last_Angle = rec_angle.load();
    int dertAng = turn(last_Angle, rec_angle.load());
    if (rec_velocity.load() > 15000)
        sum_Angle = sum_Angle.load() + dertAng / 8192.0 * 360 / 36;
    else if (rec_velocity.load() < -15000)
        sum_Angle = sum_Angle.load() + (dertAng - 8192) / 8192.0 * 360 / 36;
    else
    {
        dertAng = rec_angle.load() - last_Angle;
        if (dertAng > 4096)
            dertAng = 8191 - dertAng;
        else if (dertAng < -4096)
            dertAng = 8192 + dertAng;
        sum_Angle = sum_Angle.load() + (dertAng / 8192.0 * 360) / 36;
    }
    last_Angle = rec_angle;

    std::cout << "id: " << motor_id;
    std::cout << " sum_Angle: " << sum_Angle;
    std::cout << " velocity: " << rec_velocity << std::endl;
    // std::cout << " torque: " << rec_torque;
}
void m3508::modify(const uint8_t *data)
{
    rec_angle = ((int16_t)data[0] << 8) | (int16_t)data[1];
    rec_velocity = ((int16_t)data[2] << 8) | (int16_t)data[3];
    rec_torque = ((int16_t)data[4] << 8) | (int16_t)data[5];
    rec_temp = (int8_t)data[6];

    if (last_Angle < 0)
        last_Angle = rec_angle.load();
    int dertAng = turn(last_Angle, rec_angle.load());
    if (rec_velocity.load() > 15000)
        sum_Angle = sum_Angle.load() + dertAng / 8192.0 * 360 / 19;
    else if (rec_velocity.load() < -15000)
        sum_Angle = sum_Angle.load() + (dertAng - 8192) / 8192.0 * 360 / 19;
    else
    {
        dertAng = rec_angle.load() - last_Angle;
        if (dertAng > 4096)
            dertAng = 8191 - dertAng;
        else if (dertAng < -4096)
            dertAng = 8192 + dertAng;
        sum_Angle = sum_Angle.load() + (dertAng / 8192.0 * 360) / 19;
    }
    last_Angle = rec_angle;

    std::cout << "id: " << motor_id;
    std::cout << " sum_Angle: " << sum_Angle;
    std::cout << " velocity: " << rec_velocity << std::endl;
    // std::cout << " torque: " << rec_torque;
}
void m6020::modify(const uint8_t *data)
{
    rec_angle = ((int16_t)data[0] << 8) | (int16_t)data[1];
    rec_velocity = ((int16_t)data[2] << 8) | (int16_t)data[3];
    rec_torque = ((int16_t)data[4] << 8) | (int16_t)data[5];
    rec_temp = (int8_t)data[6];
    sum_Angle = rec_angle.load();
}
CAN_BUS::CAN_BUS(string can_device, int baudrate) : io(), t(io, boost::asio::chrono::milliseconds(1))
{
}
DJI_CAN_BUS::DJI_CAN_BUS(string can_device, int baudrate) : CAN_BUS(can_device, baudrate)
{
    can_ = new CAN(can_device, baudrate);
    for (int i = 1; i <= 8; i++)
    {
        motor_list.push_back(new m2006(i));
    }
    can_->set_Callback([this](const can_frame &frame)
                       {
            // std::cout << "in Callback" << std::endl;
            int id = frame.can_id-0x200;
            motor_list[id-1]->modify(frame.data); });

    send_frame_1.can_id = 0x200;
    send_frame_1.can_dlc = 8;
    send_frame_2.can_id = 0x1FF;
    send_frame_2.can_dlc = 8;

    running_ = true;
    t.async_wait(std::bind(&DJI_CAN_BUS::send_handler, this, std::placeholders::_1));
    io_thread = std::thread([this]
                            { io.run(); });
}
m6020_CAN_BUS::m6020_CAN_BUS(string can_device, int baudrate) : CAN_BUS(can_device, baudrate)
{
    can_ = new CAN(can_device, baudrate);
    for (int i = 1; i < 8; i++)
    {
        motor_list.push_back(new m6020(i));
    }
    can_->set_Callback([this](const can_frame &frame)
                       {
            int id = frame.can_id-0x204;
            motor_list[id-1]->modify(frame.data); });

    send_frame_1.can_id = 0x1FE;
    send_frame_1.can_dlc = 8;
    send_frame_2.can_id = 0x2FE;
    send_frame_2.can_dlc = 8;

    running_ = true;
    t.async_wait(std::bind(&m6020_CAN_BUS::send_handler, this, std::placeholders::_1));
    io_thread = std::thread([this]
                            { io.run(); });
}



void DJI_CAN_BUS::send_handler(const boost::system::error_code &e)
{
    if (!e && running_)
    {
        int16_t data; //[4] = {0, 0, 0, 0};
        for (int i = 0; i < 4; i++)
        {
            motor_list[i]->run_PID();
            data = motor_list[i]->get_current();
            send_frame_1.data[i << 1] = data >> 8;
            send_frame_1.data[(i << 1) | 1] = data & 0xFF;
        }
        // std::cout << "send: " << motor_list[0]->get_current() << std::endl;
        can_->send(send_frame_1);
        for (int i = 4; i < 8; i++)
        {
            motor_list[i]->run_PID();
            data = motor_list[i]->get_current();
            send_frame_2.data[i - 4 << 1] = data >> 8;
            send_frame_2.data[(i - 4 << 1) | 1] = data & 0xFF;
        }
        can_->send(send_frame_2);
        // 重新启动定时器
        t.expires_after(boost::asio::chrono::milliseconds(1));
        t.async_wait(std::bind(&DJI_CAN_BUS::send_handler, this, std::placeholders::_1));
    }
    else
    {
        std::cerr << "Timer error: " << e.message() << std::endl;
    }
}
void m6020_CAN_BUS::send_handler(const boost::system::error_code &e)
{
    if (!e && running_)
    {
        int16_t data; //[4] = {0, 0, 0, 0};
        for (int i = 0; i < 4; i++)
        {
            motor_list[i]->run_PID();
            data = motor_list[i]->get_current();
            send_frame_1.data[i << 1] = data >> 8;
            send_frame_1.data[(i << 1) | 1] = data & 0xFF;
        }
        // std::cout << "send: " << motor_list[0]->get_current() << std::endl;
        can_->send(send_frame_1);
        for (int i = 4; i < 7; i++)
        {
            motor_list[i]->run_PID();
            data = motor_list[i]->get_current();
            send_frame_2.data[i - 4 << 1] = data >> 8;
            send_frame_2.data[(i - 4 << 1) | 1] = data & 0xFF;
        }
        can_->send(send_frame_2);
        // 重新启动定时器
        t.expires_after(boost::asio::chrono::milliseconds(1));
        t.async_wait(std::bind(&m6020_CAN_BUS::send_handler, this, std::placeholders::_1));
    }
    else
    {
        std::cerr << "Timer error: " << e.message() << std::endl;
    }
}
void CAN_BUS::insert(int id, MOTOR_TYPE type)
{
    delete motor_list[id];
    if (type == MOTOR_2006)
        motor_list[id] = new m2006(id);
    else if (type == MOTOR_3508)
        motor_list[id] = new m3508(id);
    else
        motor_list[id] = new m2006(id);
}

DJI_CAN_BUS::~DJI_CAN_BUS()
{
    running_ = false;
    if (io_thread.joinable())
    {
        io_thread.join();
    }

    try
    {
        memset(&send_frame_1.data, 0, sizeof(send_frame_1.data));
        if (can_ && can_->is_running())
        {
            can_->send(send_frame_1);
        }

        memset(&send_frame_2.data, 0, sizeof(send_frame_2.data));
        if (can_ && can_->is_running())
        {
            can_->send(send_frame_2);
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        std::cout << "delete can_" << std::endl;
        delete can_;
        std::cout << "delete CAN_BUS" << std::endl;
        for (const auto motor : motor_list)
        {
            delete motor;
        }
        std::cout << "delete finished" << std::endl;
    }
    catch (const std::exception &e)
    {
        std::cerr << "Exception in destructor: " << e.what() << std::endl;
    }
}


m6020_CAN_BUS::~m6020_CAN_BUS()
{
    running_ = false;
    if (io_thread.joinable())
    {
        io_thread.join();
    }

    try
    {
        memset(&send_frame_1.data, 0, sizeof(send_frame_1.data));
        if (can_ && can_->is_running())
        {
            can_->send(send_frame_1);
        }

        memset(&send_frame_2.data, 0, sizeof(send_frame_2.data));
        if (can_ && can_->is_running())
        {
            can_->send(send_frame_2);
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        std::cout << "delete can_" << std::endl;
        delete can_;
        std::cout << "delete CAN_BUS" << std::endl;
        for (const auto motor : motor_list)
        {
            delete motor;
        }
        std::cout << "delete finished" << std::endl;
    }
    catch (const std::exception &e)
    {
        std::cerr << "Exception in destructor: " << e.what() << std::endl;
    }
}
